#include "IntegralImage.h"

float ** estimateDepthEdge(IntegralImage <float> ii_depth, IntegralImage <float> ii_valid_depth,int size,int width,int height);
float ** estimateNormalEdge(IntegralImage <float> ii_norm_x, IntegralImage <float> ii_norm_y, IntegralImage <float> ii_norm_z,IntegralImage <float> ii_valid_normal, int size,int width,int height);
float ** estimateRGBEdge(IntegralImage <int> ii_r, IntegralImage <int> ii_g, IntegralImage <int> ii_b, int size,int width,int height);

float ** estimateRGBEdge(IntegralImage <int> ii_r, IntegralImage <int> ii_g, IntegralImage <int> ii_b, int size,int width,int height)
{
	float ** edge = new float *[width];
	for(int i = 0; i < width; i++)
	{
		edge[i] = new float[height];
		for(int j = 0; j < height; j++)
		{
			edge[i][j] = -1;
		}
	}
	
	for(int i = size; i < width-size; i++)
	{
		for(int j = size; j < height-size; j++)
		{
			int ipsize 			= i+size;
			int jpsize 			= j+size;
			int insize 			= i-size;
			int jnsize 			= j-size;
			
			float px_norm_x		= ii_r.sum(			ipsize,	jpsize,	i,		jnsize);
			float px_norm_y 	= ii_g.sum(			ipsize,	jpsize,	i,		jnsize);
			float px_norm_z 	= ii_b.sum(			ipsize,	jpsize,	i,		jnsize);

			
			float nx_norm_x 	= ii_r.sum(			i,		jpsize,	insize,	jnsize);
			float nx_norm_y 	= ii_g.sum(			i,		jpsize,	insize,	jnsize);
			float nx_norm_z 	= ii_b.sum(			i,		jpsize,	insize,	jnsize);

			
			float dx_norm_x		= fabs(px_norm_x-nx_norm_x);
			float dx_norm_y		= fabs(px_norm_y-nx_norm_y);
			float dx_norm_z		= fabs(px_norm_z-nx_norm_z);
			
			float py_norm_x 	= ii_r.sum(			ipsize,	jpsize,	insize,	j);
			float py_norm_y 	= ii_g.sum(			ipsize,	jpsize,	insize,	j);
			float py_norm_z 	= ii_b.sum(			ipsize,	jpsize,	insize,	j);
			
			float ny_norm_x		= ii_r.sum(			ipsize,	j,		insize,	jnsize);
			float ny_norm_y		= ii_g.sum(			ipsize,	j,		insize,	jnsize);
			float ny_norm_z		= ii_b.sum(			ipsize,	j,		insize,	jnsize);

			float dy_norm_x		= fabs(py_norm_x-ny_norm_x);
			float dy_norm_y		= fabs(py_norm_y-ny_norm_y);
			float dy_norm_z		= fabs(py_norm_z-ny_norm_z);
			
			edge[i][j] 			= sqrt(dx_norm_x*dx_norm_x+dx_norm_y*dx_norm_y+dx_norm_z*dx_norm_z + dy_norm_x*dy_norm_x+dy_norm_y*dy_norm_y+dy_norm_z*dy_norm_z);
		}
	}
	return edge;
}

float ** estimateDepthEdge(IntegralImage <float> ii_depth, IntegralImage <float> ii_valid_depth,int size,int width,int height)
{
	float ** edge = new float *[width];
	for(int i = 0; i < width; i++)
	{
		edge[i] = new float[height];
		for(int j = 0; j < height; j++)
		{
			edge[i][j] = -1;
		}
	}
	
	
	for(int i = size; i < width-size; i++)
	{
		for(int j = size; j < height-size; j++)
		{
			int ipsize = i+size;
			int jpsize = j+size;
			int insize = i-size;
			int jnsize = j-size;
			
			float px 			= ii_depth.sum(			ipsize,	jpsize,	i,		jnsize);
			float divpx 		= ii_valid_depth.sum(	ipsize,	jpsize,	i,		jnsize);
			if(divpx > 0)		{px /= divpx;}
			
			float nx 			= ii_depth.sum(			i,		jpsize,	insize,	jnsize);
			float divnx 		= ii_valid_depth.sum(	i,		jpsize,	insize,	jnsize);
			if(divnx > 0)		{nx /= divnx;}
			
			float dx 			= fabs(px-nx);

			
			float py 			= ii_depth.sum(			ipsize,	jpsize,	insize,	j);
			float divpy 		= ii_valid_depth.sum(	ipsize,	jpsize,	insize,	j);
			if(divpy > 0)		{py /= divpy;}
			
			float ny 			= ii_depth.sum(			ipsize,	j,		insize,	jnsize);
			float divny 		= ii_valid_depth.sum(	ipsize,	j,		insize,	jnsize);
			if(divny > 0)		{ny /= divny;}
			
			float dy 			= fabs(py-ny);
			float div 			= ii_valid_depth.sum(	ipsize,	jpsize,	insize,	jnsize);
			float total 		= ii_depth.sum(			ipsize,	jpsize,	insize,	jnsize);
			if(divpx < size || divnx < size || divpy < size || divny < size){
				edge[i][j] 		= -1;
			}else{
				edge[i][j] 		= sqrt(dx*dx+dy*dy)/(total/div);
			}
		}
	}
	return edge;
}

float ** estimateDepthEdgev2(IntegralImage <float> ii_depth, IntegralImage <float> ii_valid_depth,int mid_size, int size ,int width,int height)
{
struct timeval start, end;
gettimeofday(&start, NULL);

	printf("v12\n");
	float ** edge = new float *[width];
	for(int i = 0; i < width; i++)
	{
		edge[i] = new float[height];
		for(int j = 0; j < height; j++)
		{
			edge[i][j] = 0;
		}
	}
	
	float scale = (float(mid_size)+0.5f*float(size))/float(size);
	
	for(int i = 2*size+mid_size; i < width-(2*size+mid_size); i++)
	{
		for(int j = 2*size+mid_size; j < height-(2*size+mid_size); j++)
		{			
			float a_valid 	= ii_valid_depth.sum(i+size,	j+mid_size+2*size	,	i-size	,	j+mid_size+1*size);
			float b_valid 	= ii_valid_depth.sum(i+size,	j+mid_size+1*size	,	i-size	,	j+mid_size+0*size);
			float c_valid 	= ii_valid_depth.sum(i+size,	j+mid_size			,	i-size	,	j-mid_size);
			float d_valid 	= ii_valid_depth.sum(i+size,	j-mid_size-0*size	,	i-size	,	j-mid_size-1*size);
			float e_valid 	= ii_valid_depth.sum(i+size,	j-mid_size-1*size	,	i-size	,	j-mid_size-2*size);

			float A_valid 	= ii_valid_depth.sum(i+mid_size+2*size	,	j+size	,	i+mid_size+1*size	,	j-size);
			float B_valid 	= ii_valid_depth.sum(i+mid_size+1*size	,	j+size	,	i+mid_size+0*size	,	j-size);
			float C_valid 	= ii_valid_depth.sum(i+mid_size			,	j+size	,	i-mid_size			,	j-size);
			float D_valid 	= ii_valid_depth.sum(i-mid_size-0*size	,	j+size	,	i-mid_size-1*size	,	j-size);
			float E_valid 	= ii_valid_depth.sum(i-mid_size-1*size	,	j+size	,	i-mid_size-2*size	,	j-size);
			
			if(a_valid > 0 && b_valid > 0 && c_valid > 0 && d_valid > 0 && e_valid > 0 && A_valid > 0 && B_valid > 0 && C_valid > 0 && D_valid > 0 && E_valid > 0){
				float a_sum 	= ii_depth.sum(i+size,	j+mid_size+2*size	,	i-size	,	j+mid_size+1*size);
				float b_sum 	= ii_depth.sum(i+size,	j+mid_size+1*size	,	i-size	,	j+mid_size+0*size);
				float c_sum 	= ii_depth.sum(i+size,	j+mid_size			,	i-size	,	j-mid_size);
				float d_sum 	= ii_depth.sum(i+size,	j-mid_size-0*size	,	i-size	,	j-mid_size-1*size);
				float e_sum 	= ii_depth.sum(i+size,	j-mid_size-1*size	,	i-size	,	j-mid_size-2*size);
				float a			= a_sum/a_valid;
				float b			= b_sum/b_valid;
				float c			= c_sum/c_valid;
				float d			= d_sum/d_valid;
				float e			= e_sum/e_valid;

				float A_sum 	= ii_depth.sum(i+mid_size+2*size	,	j+size	,	i+mid_size+1*size	,	j-size);
				float B_sum 	= ii_depth.sum(i+mid_size+1*size	,	j+size	,	i+mid_size+0*size	,	j-size);
				float C_sum 	= ii_depth.sum(i+mid_size			,	j+size	,	i-mid_size			,	j-size);
				float D_sum 	= ii_depth.sum(i-mid_size-0*size	,	j+size	,	i-mid_size-1*size	,	j-size);
				float E_sum 	= ii_depth.sum(i-mid_size-1*size	,	j+size	,	i-mid_size-2*size	,	j-size);
				float A			= A_sum/A_valid;
				float B			= B_sum/B_valid;
				float C			= C_sum/C_valid;
				float D			= D_sum/D_valid;
				float E			= E_sum/E_valid;
				
				//float ab 	= (a_sum+b_sum)/(a_valid+b_valid);
				//float de 	= (d_sum+e_sum)/(d_valid+e_valid);
				//float ab_de = fabs(ab-de);
				float a_b	= a - b;
				float d_e	= d - e;
				
				float tot = fabs(fabs(b - a_b * scale - c)-fabs(d + d_e * scale - c));
				//printf("------------------------------------------------------\n");
				//printf("ab = %f,de = %f\n",ab,de);
				//printf("b = %f, c = %f, d = %f\n",b,c,d);
				//printf("c_top = %f,c_bot = %f, diff_top = %f, diff_bot = %f\n",c_top,c_bot,diff_c_top,diff_c_bot);
				float a_b_d_e 	= a_b-d_e;
				edge[i][j] += a_b_d_e*a_b_d_e/(tot+0.015);
				//edge[i][j] += ab_de*ab_de;
				
				//float AB 	= (A_sum+B_sum)/(A_valid+B_valid);
				//float DE 	= (D_sum+E_sum)/(D_valid+E_valid);
				//float AB_DE = fabs(AB-DE);
				
				float A_B	= A  - B;
				float D_E	= D  - E;
				float A_B_D_E 	= A_B-D_E;
				float TOT = fabs(fabs(B - A_B * scale - C)-fabs(D + D_E * scale - C));
				edge[i][j] += A_B_D_E*A_B_D_E/(TOT+0.015);
				//edge[i][j] += AB_DE*AB_DE;
				edge[i][j] = sqrt(sqrt(edge[i][j]));
			}
			
		}
	}
	
gettimeofday(&end, NULL);
printf("edge estimation time: %f\n",(end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec))/1000000.0f);
	return edge;
}

float ** estimateDepthEdgev3(IntegralImage <float> ii_depth, IntegralImage <float> ii_valid_depth, int size ,int width,int height)
{
struct timeval start, end;
gettimeofday(&start, NULL);

	printf("v13\n");
	float ** edge = new float *[width];
	for(int i = 0; i < width; i++)
	{
		edge[i] = new float[height];
		for(int j = 0; j < height; j++)
		{
			edge[i][j] = -1;
		}
	}
	
	float w_valid[5];
	float w_sum[5];
	float w_value[5];
	float w_first[4];
	float w_second[3];
	
	float h_valid[5];
	float h_sum[5];
	float h_value[5];
	float h_first[4];
	float h_second[3];
	
	for(int i = 5*size; i < width-5*size; i++)
	{
		for(int j = 5*size; j < height-5*size; j++)
		{	
			w_valid[0] 	= ii_valid_depth.sum(i+3*size,	j+5*size	,	i-3*size	,	j+3*size)/3.0f;		
			w_valid[1] 	= ii_valid_depth.sum(i+2*size,	j+3*size	,	i-2*size	,	j+1*size)/2.0f;
			w_valid[2] 	= ii_valid_depth.sum(i+1*size,	j+1*size	,	i-1*size	,	j-1*size)/1.0f;
			w_valid[3] 	= ii_valid_depth.sum(i+2*size,	j-1*size	,	i-2*size	,	j-3*size)/2.0f;
			w_valid[4] 	= ii_valid_depth.sum(i+3*size,	j-3*size	,	i-3*size	,	j-5*size)/3.0f;
			
			h_valid[0] 	= ii_valid_depth.sum(i+5*size,	j+3*size	,	i+3*size	,	j-3*size)/3.0f;		
			h_valid[1] 	= ii_valid_depth.sum(i+3*size,	j+2*size	,	i+1*size	,	j-2*size)/2.0f;
			h_valid[2] 	= ii_valid_depth.sum(i+1*size,	j+1*size	,	i-1*size	,	j-1*size)/1.0f;
			h_valid[3] 	= ii_valid_depth.sum(i-1*size,	j+2*size	,	i-3*size	,	j-2*size)/2.0f;
			h_valid[4] 	= ii_valid_depth.sum(i-3*size,	j+3*size	,	i-5*size	,	j-3*size)/3.0f;
			//printf("%f, %f, %f, %f, %f\n",valid[0],valid[1],valid[2],valid[3],valid[4]);
			if(w_valid[0] != 0 && w_valid[1] != 0 && w_valid[2] != 0 && w_valid[3] != 0 && w_valid[4] != 0 && h_valid[0] != 0 && h_valid[1] != 0 && h_valid[2] != 0 && h_valid[3] != 0 && h_valid[4] != 0){
				w_sum[0] 	= ii_depth.sum		(i+3*size,	j+5*size	,	i-3*size	,	j+3*size)/3.0f;		
				w_sum[1] 	= ii_depth.sum		(i+2*size,	j+3*size	,	i-2*size	,	j+1*size)/2.0f;
				w_sum[2] 	= ii_depth.sum		(i+1*size,	j+1*size	,	i-1*size	,	j-1*size)/1.0f;
				w_sum[3] 	= ii_depth.sum		(i+2*size,	j-1*size	,	i-2*size	,	j-3*size)/2.0f;
				w_sum[4] 	= ii_depth.sum		(i+3*size,	j-3*size	,	i-3*size	,	j-5*size)/3.0f;
				
				h_sum[0] 	= ii_depth.sum		(i+5*size,	j+3*size	,	i+3*size	,	j-3*size)/3.0f;
				h_sum[1] 	= ii_depth.sum		(i+3*size,	j+2*size	,	i+1*size	,	j-2*size)/2.0f;
				h_sum[2] 	= ii_depth.sum		(i+1*size,	j+1*size	,	i-1*size	,	j-1*size)/1.0f;
				h_sum[3] 	= ii_depth.sum		(i-1*size,	j+2*size	,	i-3*size	,	j-2*size)/2.0f;
				h_sum[4] 	= ii_depth.sum		(i-3*size,	j+3*size	,	i-5*size	,	j-3*size)/3.0f;
			
				w_value[0] = w_sum[0]/w_valid[0];
				w_value[1] = w_sum[1]/w_valid[1];
				w_value[2] = w_sum[2]/w_valid[2];
				w_value[3] = w_sum[3]/w_valid[3];
				w_value[4] = w_sum[4]/w_valid[4];
				
				h_value[0] = h_sum[0]/h_valid[0];
				h_value[1] = h_sum[1]/h_valid[1];
				h_value[2] = h_sum[2]/h_valid[2];
				h_value[3] = h_sum[3]/h_valid[3];
				h_value[4] = h_sum[4]/h_valid[4];
				
				w_first[0] = w_value[0]-w_value[1];
				w_first[1] = w_value[1]-w_value[2];
				w_first[2] = w_value[2]-w_value[3];
				w_first[3] = w_value[3]-w_value[4];
				
				h_first[0] = h_value[0]-h_value[1];
				h_first[1] = h_value[1]-h_value[2];
				h_first[2] = h_value[2]-h_value[3];
				h_first[3] = h_value[3]-h_value[4];
				
				w_second[0] = w_first[0]-w_first[1];
				w_second[1] = w_first[1]-w_first[2];
				w_second[2] = w_first[2]-w_first[3];
				
				h_second[0] = h_first[0]-h_first[1];
				h_second[1] = h_first[1]-h_first[2];
				h_second[2] = h_first[2]-h_first[3];
				/*
				printf("---------------------------------\n");
				printf("value:  %f %f %f %f %f\n",value[0],value[1],value[2],value[3],value[4]);
				printf("first:  %f %f %f %f\n",first[0],first[1],first[2],first[3]);
				printf("second: %f %f %f\n",second[0],second[1],second[2]);
				printf("| %f - %f | * 1 / %f = %f\n",second[0],second[2],second[1], fabs(second[0]-second[2])/fabs(second[1]));
				*/
				
				const float limit = 100.05f;
				
				if		(h_second[0] > limit){h_second[0] = limit;}
				else if	(h_second[0] <-limit){h_second[0] =-limit;}
				if		(h_second[1] > limit){h_second[1] = limit;}
				else if	(h_second[1] <-limit){h_second[1] =-limit;}
				if		(h_second[2] > limit){h_second[2] = limit;}
				else if	(h_second[2] <-limit){h_second[2] =-limit;}
				
				if		(w_second[0] > limit){w_second[0] = limit;}
				else if	(w_second[0] <-limit){w_second[0] =-limit;}
				if		(w_second[1] > limit){w_second[1] = limit;}
				else if	(w_second[1] <-limit){w_second[1] =-limit;}
				if		(w_second[2] > limit){w_second[2] = limit;}
				else if	(w_second[2] <-limit){w_second[2] =-limit;}
				
				//const float limit2 = 0.01f;
				
				edge[i][j]= 0;
				
				
				//edge[i][j]+= fabs(w_second[0]*w_second[2])*fabs(w_second[0]*w_second[2])*fabs(w_second[0]-w_second[2])/(fabs(w_second[1])+limit2);
				//edge[i][j]+= fabs(h_second[0]*h_second[2])*fabs(h_second[0]*h_second[2])*fabs(h_second[0]-h_second[2])/(fabs(h_second[1])+limit2);
				
				
				//edge[i][j] = fabs((w_value[1]+w_first[0] - w_value[2])*(w_value[3]-w_first[3] - w_value[2]));
				
				float w_val12 = (w_sum[0]+w_sum[1])/(w_valid[0]+w_valid[1]);
				float w_val45 = (w_sum[3]+w_sum[4])/(w_valid[3]+w_valid[4]);
				float w_deriv = w_val12-w_val45;
				
				float h_val12 = (h_sum[0]+h_sum[1])/(h_valid[0]+h_valid[1]);
				float h_val45 = (h_sum[3]+h_sum[4])/(h_valid[3]+h_valid[4]);
				float h_deriv = h_val12-h_val45;
				float var = 0.003f;
				edge[i][j] += /*(fabs(w_second[1])+limit2)*/exp(-w_deriv*w_deriv/var);//*fabs(w_second[1]);//(fabs(w_second[0])+fabs(w_second[2])+limit2);
				edge[i][j] += /*(fabs(h_second[1])+limit2)*/exp(-h_deriv*h_deriv/var);
				edge[i][j] = sqrt(edge[i][j]);
			}
		}
	}
	
gettimeofday(&end, NULL);
printf("edge estimation time: %f\n",(end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec))/1000000.0f);
	return edge;
}

float ** estimateEdge(IntegralImage <float> ii_val, int size ,int width,int height);
float ** estimateEdge(float ** val, int size ,int width,int height)
{
	struct timeval start, end;
	gettimeofday(&start, NULL);
	IntegralImage <float> ii_val;
	ii_val.setup(val,width,height);
	float ** ret = estimateEdge(ii_val, size , width, height);
	ii_val.clear();
	gettimeofday(&end, NULL);
	printf("edge estimation time: %f\n",(end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec))/1000000.0f);
	return ret;
}



float ** estimateEdge(IntegralImage <float> ii_val,IntegralImage <float> ii_valid, int size ,int width,int height);
float ** estimateEdge(float ** val,float ** valid, int size ,int width,int height)
{
	struct timeval start, end;
	gettimeofday(&start, NULL);
	IntegralImage <float> ii_val;
	IntegralImage <float> ii_valid;
	ii_val.setup(val,width,height);
	ii_valid.setup(valid,width,height);
	float ** ret = estimateEdge(ii_val,ii_valid, size , width, height);
	ii_val.clear();
	ii_valid.clear();
	gettimeofday(&end, NULL);
	printf("edge estimation time: %f\n",(end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec))/1000000.0f);
	return ret;
}

float ** estimateEdge(vector< IntegralImage <float> > ii_vals, IntegralImage <float> ii_valid, int size ,int width,int height);
float ** estimate3DEdge(float ** z,float ** z_valid , float ** nx ,float ** ny ,float ** nz ,float ** n_valid ,int width,int height)
{
	struct timeval start, end;
	gettimeofday(&start, NULL);
	IntegralImage <float> ii_z;
	IntegralImage <float> ii_z_valid;
	IntegralImage <float> ii_nx;
	IntegralImage <float> ii_ny;
	IntegralImage <float> ii_nz;
	IntegralImage <float> ii_n_valid;
	ii_z.setup(z,width,height);
	ii_z_valid.setup(z_valid,width,height);
	ii_nx.setup(nx,width,height);
	ii_ny.setup(ny,width,height);
	ii_nz.setup(nz,width,height);
	ii_n_valid.setup(n_valid,width,height);
	//float ** thin = estimateEdge(ii_z,ii_z_valid, 1 , width, height);
	//float ** thick = estimateEdge(ii_z,ii_z_valid, 4 , width, height);
	
	vector< IntegralImage <float> > ii_norms;
	ii_norms.push_back(ii_nx);
	ii_norms.push_back(ii_ny);
	ii_norms.push_back(ii_nz);
	float ** normal_thin = estimateEdge(ii_norms,ii_z_valid, 2 , width, height);
	ii_z.clear();
	ii_z_valid.clear();
	ii_nx.clear();
	ii_ny.clear();
	ii_nz.clear();
	ii_n_valid.clear();
	gettimeofday(&end, NULL);
	printf("edge estimation time: %f\n",(end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec))/1000000.0f);
	return normal_thin;
}

float ** estimateEdge(IntegralImage <float> ii_val, int size ,int width,int height)
{

	float ** edge = new float *[width];
	for(int i = 0; i < width; i++)
	{
		edge[i] = new float[height];
		for(int j = 0; j < height; j++)
		{
			edge[i][j] = -1;
		}
	}
	

	int triangle 	= 2;
	for(int i = 2*triangle*size; i < width-2*triangle*size; i++)
	{
		for(int j = 2*triangle*size; j < height-2*triangle*size; j++)
		{
			float w_sum_top = 0;
			float w_sum_bot = 0;
			
			float h_sum_top = 0;
			float h_sum_bot = 0;
			for(int k = 0; k < triangle; k++){
				w_sum_top += ii_val.sum		(i+(k+1)*size	,	j-2*(k+1)*size	,	i-(k+1)*size	,	j-2*k*size);
				w_sum_bot += ii_val.sum		(i+(k+1)*size	,	j+2*k*size		,	i-(k+1)*size	,	j+2*(k+1)*size);
				
				h_sum_top += ii_val.sum		(i+2*(k+1)*size	,	j+(k+1)*size	,	i+2*k*size		,	j-(k+1)*size);
				h_sum_bot += ii_val.sum		(i-2*k*size	 	,	j+(k+1)*size	,	i-2*(k+1)*size	,	j-(k+1)*size);
			}
			
			edge[i][j] = sqrt((h_sum_top-h_sum_bot)*(h_sum_top-h_sum_bot) + (w_sum_top-w_sum_bot)*(w_sum_top-w_sum_bot));
		}
	}
	
	return edge;
}

float ** estimateEdge(vector< IntegralImage <float> > ii_vals, IntegralImage <float> ii_valid, int size ,int width,int height)
{

	float ** edge = new float *[width];
	for(int i = 0; i < width; i++)
	{
		edge[i] = new float[height];
		for(int j = 0; j < height; j++)
		{
			edge[i][j] = -1;
		}
	}
	
	int triangle 	= 2;
	for(int i = 2*triangle*size; i < width-2*triangle*size; i++)
	{
		for(int j = 2*triangle*size; j < height-2*triangle*size; j++)
		{
			edge[i][j] = 0;
			float w_valid_top = 0;
			float w_valid_bot = 0;
			float h_valid_top = 0;
			float h_valid_bot = 0;
			for(int k = 0; k < triangle; k++){
				w_valid_top += ii_valid.sum		(i+(k+1)*size	,	j-2*(k+1)*size	,	i-(k+1)*size	,	j-2*k*size);
				w_valid_bot -= ii_valid.sum		(i+(k+1)*size	,	j+2*k*size		,	i-(k+1)*size	,	j+2*(k+1)*size);
				h_valid_top += ii_valid.sum		(i+2*(k+1)*size	,	j+(k+1)*size	,	i+2*k*size		,	j-(k+1)*size);
				h_valid_bot -= ii_valid.sum		(i-2*k*size	 	,	j+(k+1)*size	,	i-2*(k+1)*size	,	j-(k+1)*size);
			}
			if(w_valid_top != 0 && w_valid_bot != 0 && h_valid_top != 0 && h_valid_bot != 0){
				for(unsigned int q = 0; q < ii_vals.size(); q++)
				{
					IntegralImage <float> ii_val = ii_vals.at(q);
					
					float w_sum_top = 0;
					float w_sum_bot = 0;
					float h_sum_top = 0;
					float h_sum_bot = 0;
			
					for(int k = 0; k < triangle; k++){
						w_sum_top 	+= ii_val.sum		(i+(k+1)*size	,	j-2*(k+1)*size	,	i-(k+1)*size	,	j-2*k*size);
						w_sum_bot 	-= ii_val.sum		(i+(k+1)*size	,	j+2*k*size		,	i-(k+1)*size	,	j+2*(k+1)*size);
						h_sum_top 	+= ii_val.sum		(i+2*(k+1)*size	,	j+(k+1)*size	,	i+2*k*size		,	j-(k+1)*size);
						h_sum_bot 	-= ii_val.sum		(i-2*k*size	 	,	j+(k+1)*size	,	i-2*(k+1)*size	,	j-(k+1)*size);
					}
					w_sum_top/=w_valid_top;
					w_sum_bot/=w_valid_bot;
			
					h_sum_top/=h_valid_top;
					h_sum_bot/=h_valid_bot;
			
					edge[i][j] += (h_sum_top-h_sum_bot)*(h_sum_top-h_sum_bot) + (w_sum_top-w_sum_bot)*(w_sum_top-w_sum_bot);
				}
				edge[i][j] = sqrt(edge[i][j]);
			}else{
				edge[i][j] = -1;
			}
		}
	}
	
	return edge;
}

float ** estimateEdge(IntegralImage <float> ii_val, IntegralImage <float> ii_valid, int size ,int width,int height)
{

	float ** edge = new float *[width];
	for(int i = 0; i < width; i++)
	{
		edge[i] = new float[height];
		for(int j = 0; j < height; j++)
		{
			edge[i][j] = -1;
		}
	}
	

	int triangle 	= 2;
	for(int i = 2*triangle*size; i < width-2*triangle*size; i++)
	{
		for(int j = 2*triangle*size; j < height-2*triangle*size; j++)
		{
			float w_sum_top = 0;
			float w_valid_top = 0;
			
			float w_sum_bot = 0;
			float w_valid_bot = 0;
			
			float h_sum_top = 0;
			float h_valid_top = 0;
			
			float h_sum_bot = 0;
			float h_valid_bot = 0;
			
			for(int k = 0; k < triangle; k++){
				w_sum_top 	+= ii_val.sum		(i+(k+1)*size	,	j-2*(k+1)*size	,	i-(k+1)*size	,	j-2*k*size);
				w_valid_top += ii_valid.sum		(i+(k+1)*size	,	j-2*(k+1)*size	,	i-(k+1)*size	,	j-2*k*size);
				w_sum_bot 	-= ii_val.sum		(i+(k+1)*size	,	j+2*k*size		,	i-(k+1)*size	,	j+2*(k+1)*size);
				w_valid_bot -= ii_valid.sum		(i+(k+1)*size	,	j+2*k*size		,	i-(k+1)*size	,	j+2*(k+1)*size);
				
				h_sum_top 	+= ii_val.sum		(i+2*(k+1)*size	,	j+(k+1)*size	,	i+2*k*size		,	j-(k+1)*size);
				h_valid_top += ii_valid.sum		(i+2*(k+1)*size	,	j+(k+1)*size	,	i+2*k*size		,	j-(k+1)*size);
				h_sum_bot 	-= ii_val.sum		(i-2*k*size	 	,	j+(k+1)*size	,	i-2*(k+1)*size	,	j-(k+1)*size);
				h_valid_bot -= ii_valid.sum		(i-2*k*size	 	,	j+(k+1)*size	,	i-2*(k+1)*size	,	j-(k+1)*size);
			}
			//printf("-----------------------------------------------\n");
			//printf("%f %f %f %f\n",w_valid_top,w_valid_bot,h_valid_top,h_valid_bot);
			//printf("%f %f %f %f\n",w_sum_top,w_sum_bot,h_sum_top,h_sum_bot);
			if(w_valid_top != 0 && w_valid_bot != 0 && h_valid_top != 0 && h_valid_bot != 0){
				w_sum_top/=w_valid_top;
				w_sum_bot/=w_valid_bot;
			
				h_sum_top/=h_valid_top;
				h_sum_bot/=h_valid_bot;
			
				edge[i][j] = sqrt((h_sum_top-h_sum_bot)*(h_sum_top-h_sum_bot) + (w_sum_top-w_sum_bot)*(w_sum_top-w_sum_bot));
			}else{
				edge[i][j] = -1;
			}
		}
	}
	
	return edge;
}

float ** estimateNormalEdge(IntegralImage <float> ii_norm_x, IntegralImage <float> ii_norm_y, IntegralImage <float> ii_norm_z,IntegralImage <float> ii_valid_normal, int size,int width,int height)
{
	float ** edge = new float *[width];
	for(int i = 0; i < width; i++)
	{
		edge[i] = new float[height];
		for(int j = 0; j < height; j++)
		{
			edge[i][j] = -1;
		}
	}

	for(int i = size; i < width-size; i++)
	{
		for(int j = size; j < height-size; j++)
		{
			int ipsize = i+size;
			int jpsize = j+size;
			int insize = i-size;
			int jnsize = j-size;
			
			float px_norm_x		= ii_norm_x.sum(			ipsize,	jpsize,	i,		jnsize);
			float px_norm_y 	= ii_norm_y.sum(			ipsize,	jpsize,	i,		jnsize);
			float px_norm_z 	= ii_norm_z.sum(			ipsize,	jpsize,	i,		jnsize);
			float divpx 		= ii_valid_normal.sum(		ipsize,	jpsize,	i,		jnsize);
			
			if(divpx > 0)		{
				float mul_divpx		= 1/divpx;
				px_norm_x *= mul_divpx;
				px_norm_y *= mul_divpx;
				px_norm_z *= mul_divpx;
			}
			
			float nx_norm_x 	= ii_norm_x.sum(			i,		jpsize,	insize,	jnsize);
			float nx_norm_y 	= ii_norm_y.sum(			i,		jpsize,	insize,	jnsize);
			float nx_norm_z 	= ii_norm_z.sum(			i,		jpsize,	insize,	jnsize);
			float divnx 		= ii_valid_normal.sum(		i,		jpsize,	insize,	jnsize);
			
			if(divnx > 0)		{
				float mul_divnx		= 1/divnx;
				nx_norm_x *= mul_divnx;
				nx_norm_y *= mul_divnx;
				nx_norm_z *= mul_divnx;
			}
			
			float dx_norm_x		= fabs(px_norm_x-nx_norm_x);
			float dx_norm_y		= fabs(px_norm_y-nx_norm_y);
			float dx_norm_z		= fabs(px_norm_z-nx_norm_z);
			
			float py_norm_x 	= ii_norm_x.sum(			ipsize,	jpsize,	insize,	j);
			float py_norm_y 	= ii_norm_y.sum(			ipsize,	jpsize,	insize,	j);
			float py_norm_z 	= ii_norm_z.sum(			ipsize,	jpsize,	insize,	j);
			float divpy 		= ii_valid_normal.sum(		ipsize,	jpsize,	insize,	j);
			
			if(divpy > 0)		{
				float mul_divpy		= 1/divpy;
				py_norm_x *= mul_divpy;
				py_norm_y *= mul_divpy;
				py_norm_z *= mul_divpy;
			}
			
			float ny_norm_x		= ii_norm_x.sum(			ipsize,	j,		insize,	jnsize);
			float ny_norm_y		= ii_norm_y.sum(			ipsize,	j,		insize,	jnsize);
			float ny_norm_z		= ii_norm_z.sum(			ipsize,	j,		insize,	jnsize);
			float divny 		= ii_valid_normal.sum(		ipsize,	j,		insize,	jnsize);
			if(divny > 0)		{
				float mul_divny		= 1/divny;
				ny_norm_x *= mul_divny;
				ny_norm_y *= mul_divny;
				ny_norm_z *= mul_divny;
			}
			
			float dy_norm_x		= fabs(py_norm_x-ny_norm_x);
			float dy_norm_y		= fabs(py_norm_y-ny_norm_y);
			float dy_norm_z		= fabs(py_norm_z-ny_norm_z);
			float div 			= 1;//ii_valid_normal.sum(		ipsize,	jpsize,	insize,	jnsize);
			if(divpx < size || divnx < size || divpy < size || divny < size){
				edge[i][j] 		= -1;
			}else{
				edge[i][j] 		= sqrt(dx_norm_x*dx_norm_x+dx_norm_y*dx_norm_y+dx_norm_z*dx_norm_z + dy_norm_x*dy_norm_x+dy_norm_y*dy_norm_y+dy_norm_z*dy_norm_z)/div;
			}
		}
	}
	return edge;
}
